package com.grt192.spot.mechanism.WallHugger;

import com.grt192.core.Mechanism;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.mechanism.event.WallHuggable;
import com.grt192.mechanism.iaroc.IRCreate;
import com.grt192.spot.sensor.GRTSonar;
import com.iaroc.irobot.IRobotCreate;
import java.util.Vector;

/**
 * A WallHugging Robot base with a single sonar sensor.
 * Sends boolean sensor events based on proximity to wall as detected by sonars.
 * @author ajc
 */
public class WallHuggingBase extends Mechanism implements SensorChangeListener {

    //actuators and sensors
    private final IRCreate irc;
    /**
     * If using a single sonar, it is placed on the right, about 45deg from heading
     */
    private final GRTSonar[] sonars;
    private IRobotCreate create;
    /** list of listeners to notify on wall hugging event (too close/far) **/
    private Vector listeners;
    /** Threshold distance reported by sonar for changing drive**/
    private double SONAR_TOO_CLOSE = 8;
    /** velocity of robot in mm/s **/
    private int VELOCITY = 117;
    /**radius of turning in mm **/
    private int SLIGHT_TURN_RIGHT = -500;
    private int HARD_TURN_LEFT = 250;

    /**
     * Constructs a WallHugging base, which has an irobot create and any number
     * of sonars. There is almost no advantage to using more than one sonar.
     * @param irc the irobot create to drive with
     * @param sonars the maxbotix sonars for wall detection
     */
    public WallHuggingBase(IRCreate irc, GRTSonar[] sonars) {
        create = irc.getCreate();
        this.irc = irc;
        this.sonars = sonars;
        //start sonars if not started already, and start listening for sensor events.
        for (int i = 0; i < sonars.length; i++) {
            if (!sonars[i].isAlive()) {
                sonars[i].start();
            }
            sonars[i].addSensorChangeListener(this);
        }
        listeners = new Vector();
    }

    /** Performs a very slight turn in one direction to constantly get walls **/
    public void driveTurn() {
        log("driveturn: \t" + VELOCITY + "\t" + SLIGHT_TURN_RIGHT);
        if (create != null) {
            create.drive(VELOCITY, SLIGHT_TURN_RIGHT);
        }
    }

    /** Performs a comparatively hard turn to avoid physically hitting walls **/
    public void turn() {
        log("turnHard: \t" + VELOCITY + "\t" + HARD_TURN_LEFT);
        if (create != null) {
            create.drive(VELOCITY, HARD_TURN_LEFT);
        }

    }

    /** Notifies all WallHuggables that the base has neared the wall **/
    private void notifyTooClose() {
        for (int i = 0; i < listeners.size(); i++) {
            ((WallHuggable) listeners.elementAt(i)).tooClose();
        }
    }

    /** Notifies all wallHuggables that the base has left the wall **/
    private void notifyTooFar() {
        for (int i = 0; i < listeners.size(); i++) {
            ((WallHuggable) listeners.elementAt(i)).tooFar();
        }
    }

    /** Adds a Wallhuggable object to notify on sensor state changes **/
    public void addWallHuggingController(WallHuggable w) {
        listeners.addElement(w);
    }

    /** Removes a Wallhuggable object to stop notifying **/
    public void removeWallHuggingController(WallHuggable w) {
        listeners.removeElement(w);
    }

    public void sensorStateChanged(SensorEvent e, String key) {
        if (e.getData("Distance") > SONAR_TOO_CLOSE) {
            notifyTooFar();
        } else {
            notifyTooClose();
        }
    }
}
